#include "sys.h"
#include "taskA.h"
#include "app.h"

float y_A[] = {70.8,70};
float x_A = 39.6,W_A=50.0,A_B0 = 1.7;
extern u16 step;
//串口1为:树莓派/jetson nano
//串口2为:机械臂stm32f103
//串口3为:左openmv	
//串口4为:右openmv
//串口5为:前stm32f407(红外对管，超声波),f4蓝牙
//串口6为:小车stm32f103

void wait_s(void)
{
	u5_printf("S1:%d\r\n", carstate);
	while(carstate!=3);
	delay_ms(20);
	u5_printf("S2:%d\r\n", carstate);
	while(carstate==3);
	u5_printf("S3:%d\r\n", carstate);
}

void TaskA(void)
{
	static u16 step_A = 0;
	static u16 else_count = 0;
	if(stop_flag == 1)
	{
		switch(step_A)
		{
			case 0:
			{
				u5_printf("-------------1------------\r\n");
				direction = 1; //右边果树			
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-A_B0)>0.3) CloseToWall(-1,-1,A_B0);
				AB0_CloseToFruit(x_A,y_A[direction]);
				step_A++;
				break;
			}
			case 1:
			{
				GrapLowFruit_AB('L');
				step_A++;
				break;
			}
			case 2:
			{
				order_num(2, 'D', 1);					//看树上
				AB1_CloseToFruit(W_A);
				GrapHighFruit_AB(1);
				step_A++;
				break;
			}
			case 3:
			{
				u5_printf("step_A = 3\r\n");	 
				else_count = 0;
				stop_flag = 0;//跳出if进入else
				step_A++;
				break;
			}
			case 4:
			{
				u5_printf("-------------2------------\r\n");
				direction = 0;//左边果树
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-A_B0+1)>0.3) CloseToWall(-1,-1,A_B0+1);
				AB0_CloseToFruit(x_A,y_A[direction]);
				step_A++;
				break;
			}
			case 5:
			{
				GrapLowFruit_AB('R');
				step_A++;
				break;
			}
			case 6:
			{
				order_num(2, 'A', 1);					//看树上
				AB1_CloseToFruit(W_A);
				GrapHighFruit_AB(1);
				step_A++;
				break;
			}
			case 7:
			{
				u5_printf("case 7\r\n");
				else_count = 0;
				stop_flag = 0;//跳出if进入else
				step_A++;
				break;
			}
			case 8:
			{
				u5_printf("-------------3------------\r\n");
				direction = 1; //右边果树		
				order(3, 'S'); //让车停下
				wait_w(3, 'S', 1);       
				u5_printf("STOP\r\n");
				u5_printf("find target\r\n");
				if(fabs(Leida_Distance[2]-A_B0+2)>0.3) CloseToWall(-1,-1,A_B0+2);
				AB0_CloseToFruit(x_A,y_A[direction]);
				step_A++;
				break;
			}
			case 9:
			{
				GrapLowFruit_AB('L');
				step_A++;
				break;
			}
			case 10:
			{
				order_num(2, 'D', 1);					//看树上
				AB1_CloseToFruit(W_A);
				GrapHighFruit_AB(1);
				step_A++;
				break;
			}
			case 11:
			{
				u5_printf("case 11\r\n");
				u5_printf("LOOK UP\r\n");
				order_num(2, 'K', 1);
				order_num(3, 'W', 0.1);
				wait_w(3, 'W', 0.1);
				u5_printf("let's go\r\n");			
				step_A++;
				else_count = 0;
				u5_printf("reset else_count:%d\r\n", else_count);
				stop_flag = 0;//跳出if进入else
				direction = 0;
				step++;
				return;
			}
		}
	}
	else
	{
		if(else_count == 0)
		{
			u5_printf("the first else\r\n");
			//这里与app.c的描述要相反
			if(direction == 0)
			{
				order_num(2, 'A', 0);
			}
			else if(direction == 1)
			{
				order_num(2, 'D', 0);
			}
			if(step_A==0) u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.25,'W',0.25);
			else u3_printf("~%c%.2f~%c%.2f\r\n",'X',1.0,'W',0.25);	//浮点数输出必须要有小数点 %f和%lf输出6位小数  
			wait_s();
			stop_flag = 0;
		}
		else
		{
			if(Fruit_num != 0)
			{
				u5_printf("Fruit_num:%d\r\n",Fruit_num);
				stop_flag = 1;
			}
		}
		else_count = 1;
	}
}

